import rclpy
from rclpy.node import Node
import apriltag
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
import cv2
import math
from sls_msgs.msg import SLSApirltag

class ApirltagDetect(Node):
    def __init__(self,source):
        super().__init__('apirltag_test_node')
        self.pub = self.create_publisher(SLSApirltag,'apirltag_coordination',10)
        self.timer_det = self.create_timer(0.1,self.timer_callback)
        # self.detector = apriltag.Detector(families = 'tag36h11')
        self.detector = apriltag.Detector()
        self.cap = cv2.VideoCapture(source)
        if not self.cap.isOpened():
            self.get_logger().error('SLS: Failed to open camera, please ensure your camera source is 2!')

    def timer_callback(self):
        self.isdetect = False
        id = -1
        ret,frame = self.cap.read()
        gray_frame = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
        results = self.detector.detect(gray_frame)
        if results != []:
            for result in results:
                (self.cX, self.cY) = (int(result.center[0]), int(result.center[1]))
                cv2.line(frame,tuple(result.corners[0].astype(int)),
                            tuple(result.corners[1].astype(int)),(0, 255, 0),10)
                cv2.line(frame,tuple(result.corners[1].astype(int)),
                            tuple(result.corners[2].astype(int)),(0, 255, 0),10)
                cv2.line(frame,tuple(result.corners[2].astype(int)),
                            tuple(result.corners[3].astype(int)),(0, 255, 0),10)
                cv2.line(frame,tuple(result.corners[3].astype(int)),
                            tuple(result.corners[0].astype(int)),(0, 255, 0),10)
                cv2.circle(frame, (self.cX, self.cY), 10, (0, 0, 255), -1)
            for result in results:
                (self.cX, self.cY) = (int(result.center[0]), int(result.center[1]))
                self.angle = math.atan((result.corners[0][1]-result.corners[3][1])/
                                    (result.corners[0][0]-result.corners[3][0]))*57.3
                id = result.tag_id
                self.isdetect = True
                if id == 10:
                    break
        else:
                self.isdetect =False
        cv2.imshow("video", frame)
        cv2.waitKey(1)
        coordinate = SLSApirltag()
        if self.isdetect == True:
            coordinate.x = self.cX
            coordinate.y = self.cY
            coordinate.angle = int(self.angle)
            coordinate.id = id
            coordinate.isdetect = self.isdetect
        else:
            self.get_logger().warning('no coordinate')
        self.pub.publish(coordinate)

def main(args=None):
    rclpy.init(args=args)
    node = ApirltagDetect(0)
    rclpy.spin(node)
    node.destroy_node(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()